package fem2.strategies;

import inf.jlinalg.IVector;
import fem2.Model;
import fem2.Node;

/**
 * Arc length solution strategy with displacement difference constraint between
 * two nodes
 * 
 * @author hbui
 * 
 */
public class ArcLengthSolutionStrategyWithDistanceConstraint extends ArcLengthSolutionStrategy {

	protected double d;
	private double[] dArray = null;
	protected Node n1;
	protected Node n2;

	private int step = 0;

	/**
	 * 
	 * @param m
	 *            model
	 * @param d
	 *            constrainted distance
	 * @param n1
	 *            free node <br>
	 *            TODO to extend it to constrainted node
	 * @param n2
	 *            free node <br>
	 *            TODO to extend it to constrainted node
	 */
	public ArcLengthSolutionStrategyWithDistanceConstraint(Model m, double d, Node n1, Node n2) {
		super(m);
		this.d = d;
		this.n1 = n1;
		this.n2 = n2;
	}

	/**
	 * 
	 * @param m
	 *            model
	 * @param dArray
	 *            array of constrainted displacement distance
	 * @param n1
	 *            free node <br>
	 *            TODO to extend it to constrainted node
	 * @param n2
	 *            free node <br>
	 *            TODO to extend it to constrainted node
	 */
	public ArcLengthSolutionStrategyWithDistanceConstraint(Model m, double[] dArray, Node n1,
			Node n2) {
		super(m);
		this.dArray = dArray;
		this.d = dArray[0];
		this.n1 = n1;
		this.n2 = n2;
		setNumberOfTimeStep(dArray.length);
	}

	@Override
	protected double getNextSteppingParameter() {
		if (dArray != null) {
			d = dArray[step++];
		}
		return d;
	}

	@Override
	protected double getCurrentSteppingParameter() {
		return d;
	}

	@Override
	protected double getScalingParameter() {
		return 0.0;
	}

	@Override
	protected double computeConstraintValue(IVector u, IVector u_old, double lambda,
			double lambda_old) {
		int[] dofs1 = n1.getDofNumbers();
		int[] dofs2 = n2.getDofNumbers();
		double d2 = 0.0;
		double d2_old = 0.0;
		for (int i = 0; i < dofs1.length; i++) {
			d2 += Math.pow(u.get(dofs2[i]) - u.get(dofs1[i]), 2);
			d2_old += Math.pow(u_old.get(dofs2[i]) - u_old.get(dofs1[i]), 2);
		}
		return Math.sqrt(d2) - Math.sqrt(d2_old) - this.d;
	}

	@Override
	protected void computeGradientOfConstraintWrtU(IVector u, IVector u_old, double lambda,
			double lambda_old, IVector dfdu) {
		int[] dofs1 = n1.getDofNumbers();
		int[] dofs2 = n2.getDofNumbers();
		double d2_old = 0.0;
		for (int i = 0; i < dofs1.length; i++) {
			d2_old += Math.pow(u_old.get(dofs2[i]) - u_old.get(dofs1[i]), 2);
		}
		double d = computeConstraintValue(u, u_old, lambda, lambda_old) + Math.sqrt(d2_old)
				+ this.d;
		for (int i = 0; i < dfdu.getSize(); i++) {
			dfdu.set(i, 0.0);
		}
		for (int i = 0; i < dofs1.length; i++) {
			dfdu.set(dofs1[i], (u.get(dofs1[i]) - u.get(dofs2[i])) / d);
		}
		for (int i = 0; i < dofs2.length; i++) {
			dfdu.set(dofs2[i], (u.get(dofs2[i]) - u.get(dofs1[i])) / d);
		}
	}

	@Override
	protected double computeDerivativeOfConstraintWrtLambda(IVector u, IVector u_old,
			double lambda, double lambda_old) {
		return 0.0;
	}

}
